#ifndef _POLYGON_MATCHER_H_
#define	_POLYGON_MATCHER_H_

/************************************************************************/
/*					Polygon Match using affine match                    */
/************************************************************************/
#include <vector>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <opencv/cv.h>
#include <opencv2/core/types_c.h>
#include <CGAL/Dimension.h>
#include <CGAL/centroid.h>
#include "cmAffine.h"
#include "../common/polytope.h"

namespace Field_packing
{

	using namespace Packing_common;

	template <class UserDataType>
	struct Match_info_item
	{
		Parameter::Transformation_2 t;
		//double scale;
		//double theta;
		//double tx;
		//double ty;
		double error;//match error
		UserDataType val;
	};

	template <class UserDataType>
	struct Match_measure : std::binary_function< const Match_info_item<UserDataType>&, const Match_info_item<UserDataType>&, bool>
	{
		inline bool operator()(const Match_info_item<UserDataType>& i0, const Match_info_item<UserDataType>& i1) const
		{
			return i0.error > i1.error;
		}
	};

	class Polygon_matcher
	{
		typedef std::vector<cv::Point2d>	PointSet; 
		typedef Packing_polygon::Point_2	Point_2;
		typedef Packing_polygon::Segment_2	Segment_2;
		typedef Packing_polygon::Vector		Vector_2;
		typedef Parameter::Transformation_2 Transformation_2;
	public:
		//Polygon_matcher(std::vector<Segment_2>& model, unsigned int _nb_sampling = 50);

		// InputIter is the iterator to a container of segments
		template <class InputIter>
		Polygon_matcher(InputIter first, InputIter last, size_t _nb_sampling = 50);

		Polygon_matcher(const CGAL_polygon& model, unsigned int _nb_sampling = 50);
		~Polygon_matcher();

		template <class UserDataType>
		Match_info_item<UserDataType> affine_match(const Packing_polygon& instance, const UserDataType& data);

		double get_model_area() const { return model_area; }

	private:
		template <class VectorType> inline void cgal_vec_normalize(VectorType& v)
		{
			v = v / CGAL::sqrt(v.squared_length());
		}

	private:
		unsigned int nb_sampling;
		PointSet modelPtsSet;
		//std::vector<Vector_2> model_tangent_vec; // tangent vectors at each sampling points
		CvMat *modelMat;
		double model_area;
	};

	template <class InputIter>
	Polygon_matcher::Polygon_matcher(InputIter first, InputIter last, size_t _nb_sampling) : nb_sampling(_nb_sampling)
	{
		model_area = 0.0;
		double perimeter = 0.0;
		std::list<double> edge_lens;
		Point_2 p = first->source();
		InputIter it;
		for (it = first; it != last; ++it)
		{
			model_area += std::fabs(CGAL::area(p, it->source(), it->target()));
			edge_lens.push_back(std::sqrt(it->squared_length()));
			perimeter += edge_lens.back();
		}
		modelPtsSet.reserve(nb_sampling);
		it = first;
		for (std::list<double>::const_iterator edge_len_it = edge_lens.begin(); it != last; ++it, ++edge_len_it)
		{
			int n = (*edge_len_it)/perimeter*nb_sampling;
			Point_2 src = it->source(), tgt = it->target();
			//Vector_2 tv(src, tgt);
			//cgal_vec_normalize(tv);

			for (size_t i = 0; i < n+1; i++)
			{
				Point_2 sp = CGAL::ORIGIN + ( (n+1-i)*(src - CGAL::ORIGIN) + i*(tgt - CGAL::ORIGIN) ) / (n+1);
				modelPtsSet.push_back(cv::Point2d(sp.x(), sp.y()));
			}
		}
		modelMat = CmAffine::CreatePointSetMat(modelPtsSet);
	}
	template <class UserDataType>
	Match_info_item<UserDataType> Polygon_matcher::affine_match(const Packing_polygon& instance_, const UserDataType& data)
	{
		Point_2 c = CGAL::centroid(instance_.vertices_begin(), instance_.vertices_end(), CGAL::Dimension_tag<0>());
		Transformation_2 to_cent(CGAL::TRANSLATION, Vector_2(c, CGAL::ORIGIN));
		CGAL_polygon instance = CGAL::transform(to_cent, instance_);
		double perimeter = 0.0;
		std::vector<double> edge_lens(instance.size());
		for (unsigned int i = 0; i < instance.size(); i++)
		{
			Segment_2 s = instance.edge(i);
			edge_lens[i] = CGAL::sqrt(s.squared_length());
			perimeter += edge_lens[i];
		}
		nb_sampling = 200;
		PointSet instancePtsSet;

		//std::vector<Vector_2> instance_tangent_vec;
		//instance_tangent_vec.reserve(nb_sampling);

		instancePtsSet.reserve(nb_sampling);
		for (unsigned int i = 0; i < instance.size(); i++)
		{
			unsigned int n = (edge_lens[i]/perimeter) * nb_sampling;
			Segment_2 s = instance.edge(i);
			Point_2 src = s.source(), tgt = s.target();

			//Vector_2 tv(src, tgt);
			//cgal_vec_normalize(tv);

			for (unsigned int j = 0; j < n+1; j++)
			{
				Point_2 sp = CGAL::ORIGIN + ( (n+1-j)*(src - CGAL::ORIGIN) + j*(tgt - CGAL::ORIGIN) ) / (n+1);
				instancePtsSet.push_back(cv::Point2d(sp.x(), sp.y()));

				//instance_tangent_vec.push_back(tv);
			}
		}
		CvMat *instanceMat = CmAffine::CreatePointSetMat(instancePtsSet);

		CvMat *AA = cvCreateMat(2, 2, CV_64FC1);
		CvMat *shift = cvCreateMat(2, 1, CV_64FC1);

		double theta =	CmAffine::GetAffine2D(instanceMat, modelMat, AA, shift);

		//double scale = std::sqrt(model_area/std::fabs(instance.area()));
		double scale = 1.0;

		double *r = AA->data.db;
		double sn = std::sin(theta), cs = std::cos(theta);
		r[0]  = scale*cs; 
		r[1] = -scale*sn;
		r[2] = -r[1];   
		r[3] = r[0];

		//Transformation_2 t(r[0], r[1], shift->data.db[0], r[2], r[3], shift->data.db[1]);

		CvMat *ptsMat = cvCreateMat(2, instancePtsSet.size(), CV_64FC1);
		CmAffine::FindTranslated(instanceMat, AA, shift, ptsMat);

		double *x = ptsMat->data.db;
		double *y = (double*)(ptsMat->data.ptr + ptsMat->step);
		for (int i = 0; i < ptsMat->cols; i++)
			instancePtsSet[i] = Point2d(x[i], y[i]);

		double mean_dist = 0.0;
		//double mean_tangent_diff = 0.0;
		for (unsigned int i = 0; i < instancePtsSet.size(); i++)
		{
			//unsigned int min_idx;
			double min_dist = std::numeric_limits<double>::max();
			for (unsigned int j = 0; j < modelPtsSet.size(); j++)
			{
				double temp = sqrDist(instancePtsSet[i], modelPtsSet[j]);
				if (min_dist > temp)
				{
					min_dist = temp;
					//min_idx = j;
				}
			}
			mean_dist += std::sqrt(min_dist);
			//mean_tangent_diff += std::fabs(instance_tangent_vec[i].x()*model_tangent_vec[min_dist].y() - instance_tangent_vec[i].y()*model_tangent_vec[min_dist].x());
		}
		mean_dist /= instancePtsSet.size();
		//mean_tangent_diff /= instancePtsSet.size();
		//mean_tangent_diff *= CGAL::sqrt(model_area);

		Match_info_item<UserDataType> res;
		//std::cout<<"( "<<mean_dist<<", "<<mean_tangent_diff<<" )\n";
		//res.error = mean_dist + lambda * mean_tangent_diff; 
		res.error = mean_dist;
		res.t = Transformation_2(cs, -sn, shift->data.db[0], sn, cs, shift->data.db[1])*to_cent;
		//res.scale = scale;
		//res.theta = theta;
		//res.tx = shift->data.db[0];
		//res.ty = shift->data.db[1];
		res.val = data;

		cvReleaseMat(&instanceMat);
		cvReleaseMat(&shift);
		cvReleaseMat(&AA);
		cvReleaseMat(&ptsMat);

		return res;
	}
}

#endif